
#include "registration.h"

void voxelCloud (pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output, double leafSize ){

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::VoxelGrid<pcl::PointXYZRGB> sor;
	sor.setInputCloud (input);
	sor.setLeafSize (leafSize,leafSize,leafSize);
	sor.filter (*cloud_filtered);

	copyPointCloud(*cloud_filtered,*output);

}

void segmentation(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr source, pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmented, float threshold){


	pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<pcl::PointXYZRGB> seg;

	seg.setOptimizeCoefficients (true);

	seg.setModelType (pcl::SACMODEL_PLANE);
	seg.setMethodType (pcl::SAC_RANSAC);
	seg.setDistanceThreshold (threshold);

	seg.setInputCloud (source);
	seg.segment (*inliers, *coefficients);

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud (source);
	extract.setIndices (inliers);
	extract.setNegative (true);

	extract.filter (*segmented);
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*segmented, *segmented, indices);

	// euclidean clustering
	typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
	tree->setInputCloud (segmented);

	std::vector<pcl::PointIndices> cluster_indices;
	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> clustering;
	clustering.setClusterTolerance (0.0002); // 2cm
	clustering.setMinClusterSize (1000);
	clustering.setMaxClusterSize (250000);
	clustering.setSearchMethod (tree);
	clustering.setInputCloud(segmented);
	clustering.extract (cluster_indices);

	if (cluster_indices.size() > 0){

		typename pcl::IndicesPtr indices (new std::vector<int>);
		*indices = cluster_indices[0].indices;
		extract.setInputCloud (segmented);
		extract.setIndices (indices);
		extract.setNegative (false);

		extract.filter (*segmented);
	}
}

void computeKeyPoints(pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcd, pcl::PointCloud<pcl::PointXYZI>::Ptr out){

	pcl::SIFTKeypoint<pcl::PointXYZRGB, pcl::PointXYZI> sift_key;
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());

	float min_scale = 0.01;
	int nr_oct = 3;
	int nr_spo = 2;
	float min_contrast = 0.0;

	sift_key.setInputCloud(pcd);
	sift_key.setScales(min_scale,nr_oct,nr_spo);
	sift_key.setMinimumContrast(0.0);
	sift_key.setSearchMethod(tree);
	pcl::PointCloud<pcl::PointXYZI>::Ptr sift_out (new pcl::PointCloud<pcl::PointXYZI>);
	sift_key.compute(*sift_out);

	copyPointCloud(*sift_out,*out);

}

void extractFeatures(pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr input, pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::FPFHSignature33>::Ptr cloud_out){

	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
	kpts->points.resize(cloud->points.size());

	pcl::copyPointCloud(*cloud, *kpts);

	// Remove NaN from the point cloud:
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*kpts, *kpts, indices);

	// Create the normal estimation class, and pass the input dataset to it
	pcl::NormalEstimationOMP<pcl::PointXYZRGB, pcl::Normal> ne;
	ne.setInputCloud (kpts);

	// Create an empty kdtree representation, and pass it to the normal estimation object.
	pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
	ne.setSearchMethod (tree);

	// Output datasets
	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

	ne.setRadiusSearch (20);

	// Compute the features
	ne.compute (*cloud_normals);

	// Create the FPFH estimation class, and pass the input dataset+normals to it
	pcl::FPFHEstimationOMP<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> fpfh;
	fpfh.setInputCloud (kpts);
	fpfh.setInputNormals (cloud_normals);

	// Output datasets
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());

	fpfh.setRadiusSearch (25);

	// Compute the features
	fpfh.compute (*fpfhs);

	pcl::copyPointCloud(*fpfhs,*cloud_out);
	return;
}

void findCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr source, typename pcl::PointCloud<pcl::FPFHSignature33>::Ptr target, std::vector<int>& correspondences){
	correspondences.resize (source->size());

	// Use a KdTree to search for the nearest matches in feature space
	pcl::search::KdTree<pcl::FPFHSignature33> descriptor_kdtree (new pcl::search::KdTree<pcl::FPFHSignature33> ());
	descriptor_kdtree.setInputCloud (target);

	// Find the index of the best match for each keypoint, and store it in "correspondences"
	const int k = 1;
	std::vector<int> k_indices (k);
	std::vector<float> k_squared_distances (k);
	for (size_t i = 0; i < source->size (); ++i){
		descriptor_kdtree.nearestKSearch (*source, i, k, k_indices, k_squared_distances);
		correspondences[i] = k_indices[0];
	}

}

void filterCorrespondences(std::vector<int>& st, std::vector<int>& ts, pcl::CorrespondencesPtr corrispondenzePtr, std::vector<std::pair<unsigned, unsigned> >& corrispondenze, pcl::PointCloud<pcl::PointXYZI>::Ptr sk, pcl::PointCloud<pcl::PointXYZI>::Ptr tk, float threshold ){

	for (unsigned cIdx = 0; cIdx < st.size (); ++cIdx)
		if (ts[st[cIdx]] == cIdx)
			corrispondenze.push_back(std::make_pair(cIdx, st[cIdx]));

	corrispondenzePtr->resize (corrispondenze.size());
	for (unsigned cIdx = 0; cIdx < corrispondenze.size(); ++cIdx){
		(*corrispondenzePtr)[cIdx].index_query = corrispondenze[cIdx].first;
		(*corrispondenzePtr)[cIdx].index_match = corrispondenze[cIdx].second;
	}

	pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZI> rejector;
	rejector.setInputCloud(sk);
	rejector.setTargetCloud(tk);
	rejector.setInlierThreshold(threshold);
	rejector.setInputCorrespondences(corrispondenzePtr);
	rejector.getCorrespondences(*corrispondenzePtr);

	return;

}

void initialAlignment(pcl::CorrespondencesPtr corrispondenzeP, pcl::PointCloud<pcl::PointXYZI>::Ptr sk, pcl::PointCloud<pcl::PointXYZI>::Ptr tk, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_out, Eigen::Matrix4f& iniziale ){
	pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>::Ptr transformation_estimation (new pcl::registration::TransformationEstimationSVD<pcl::PointXYZI, pcl::PointXYZI>);

	transformation_estimation->estimateRigidTransformation (*sk, *tk, *corrispondenzeP, iniziale);

	pcl::transformPointCloud(*s, *s_out, iniziale);
	return;
}


void finalAlignment(pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_alg, pcl::PointCloud<pcl::PointXYZRGB>::Ptr t, pcl::PointCloud<pcl::PointXYZRGB>::Ptr s_out, Eigen::Matrix4f& finale){

	pcl::Registration<pcl::PointXYZRGB, pcl::PointXYZRGB>::Ptr registration (new pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB>);
	registration->setInputCloud(s_alg);
	registration->setInputTarget (t);
	registration->setMaxCorrespondenceDistance(4);
	registration->setRANSACOutlierRejectionThreshold (4);
	registration->setTransformationEpsilon (0.000001);
	//registration->setMaximumIterations (1000);
	registration->setMaximumIterations (100);
	registration->align(*s_out);
	finale = registration->getFinalTransformation();

	return;
}

void cableDetection(pcl::PointCloud<pcl::PointXYZRGB>::Ptr area, pcl::PointCloud<pcl::PointXYZRGB>::Ptr out){

	std::vector<pcl::PointIndices> cluster_indices;
	//clusterObjects (area, 0.7, 800, 2000, cluster_indices);

	pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
	ec.setClusterTolerance (0.7);
	ec.setMinClusterSize (800);
	ec.setMaxClusterSize (2000);
	ec.setInputCloud (area);
	ec.extract (cluster_indices);

	for (int i = 0; i < cluster_indices.size (); ++i){

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster_i (new pcl::PointCloud<pcl::PointXYZRGB>);
		pcl::copyPointCloud (*area, cluster_indices[i], *cluster_i);

		pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> random_color (cluster_i, 255, 0, 0);

		for(int j=0; j < cluster_i->points.size(); j++){

			cluster_i->points[j].r = 255;
			cluster_i->points[j].g = 0;
			cluster_i->points[j].b = 0;

		}

		pcl::copyPointCloud(*cluster_i, *out);

	}

}

void selectCableArea(pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_registrata, pcl::PointCloud<pcl::PointXYZRGB>::Ptr area ){

	for(int i=0; i<source_registrata->points.size(); i++){

		//SELEZIONE AREA IN CUI E' PRESENTE IL CAVO
		if(source_registrata->points[i].y>55 && source_registrata->points[i].y<80 && source_registrata->points[i].x<-8 && source_registrata->points[i].x>-32
				&& source_registrata->points[i].z < 346){

			pcl::PointXYZRGB *p= new pcl::PointXYZRGB();
			p->x=source_registrata->points[i].x;
			p->y=source_registrata->points[i].y;
			p->z=source_registrata->points[i].z;

			p->r= source_registrata->points[i].r;
			p->g= source_registrata->points[i].g;
			p->b= source_registrata->points[i].b;

			area->points.push_back(*p);

		}
	}

}


void transformCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output){

	int dimensione = input->points.size();

	(*output).width    = dimensione;
	(*output).height   = 1;
	(*output).is_dense = false;
	(*output).points.resize (dimensione);

	for (int i = 0; i < input->points.size(); ++i){

		(*output).points[i].x = input->points[i].x;
		(*output).points[i].y = input->points[i].y;
		(*output).points[i].z = input->points[i].z;
		(*output).points[i].r = input->points[i].r;
		(*output).points[i].g = input->points[i].g;
		(*output).points[i].b = input->points[i].b;

	}

}


void pioliDetection(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr p1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr p2){

	int piolo1 = 0, piolo2 = 0;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGB>);


	for(int i=0; i<input->points.size(); i++){

		//SELEZIONE AREA IN CUI E' PRESENTE IL CAVO
		if(input->points[i].y>55 && input->points[i].y<82 && input->points[i].x<-8 && input->points[i].x>-40){
			pcl::PointXYZRGB *p= new pcl::PointXYZRGB();
			p->x=input->points[i].x;
			p->y=input->points[i].y;
			p->z=input->points[i].z;


			//CONTROLLO ALTEZZA
			if(input->points[i].z < 339.5 && input->points[i].z > 338 &&
				input->points[i].x > -30 && input->points[i].x < -19 &&
				input->points[i].y > 72 && input->points[i].y < 75.5){


				//INDIVIDUAZIONE PIOLO 1
				if(input->points[i].x > -24.5 && input->points[i].x < -19){
					p1->points.push_back(input->points[i]);
					p->r= 0;
					p->g= 255;
					p->b= 0;
					piolo1++;
				}

				//INDIVIDUAZIONE PIOLO 2
				if(input->points[i].x > -27.8 && input->points[i].x <= -24){
					p2->points.push_back(input->points[i]);
					p->r= 0;
					p->g= 255;
					p->b= 0;
					piolo2++;
				}

			}
			else{
					p->r= input->points[i].r;
					p->g= input->points[i].g;
					p->b= input->points[i].b;
			}

			cloud_out->points.push_back(*p);

		}
		else{
			pcl::PointXYZRGB *p= new pcl::PointXYZRGB();
			p->x= input->points[i].x;
			p->y= input->points[i].y;
			p->z= input->points[i].z;
			p->r= input->points[i].r;
			p->g= input->points[i].g;
			p->b= input->points[i].b;

			cloud_out->points.push_back(*p);

		}

	}

	pcl::copyPointCloud((*cloud_out), (*output));

	//AGGIUNTA DEL PIOLO 1
	if(piolo1==0 && piolo2!=0){
		for (int i=0; i<p2->points.size(); i++){
			pcl::PointXYZRGB *p= new pcl::PointXYZRGB();
			p->x= p2->points[i].x + 4.7;
			p->y= p2->points[i].y;
			p->z= 338.2;
			p->r= 0;
			p->g= 255;
			p->b= 0;
			(*output).push_back(*p);
			p1->points.push_back(*p);

		}
	}

	//AGGIUNTA DEL PIOLO 2
	if(piolo2==0 && piolo1!=0){
		for (int i=0; i<p1->points.size(); i++){
			pcl::PointXYZRGB *p= new pcl::PointXYZRGB();
			p->x= p1->points[i].x - 4.7;
			p->y= p1->points[i].y;
			p->z= 339.2;
			p->r= 0;
			p->g= 255;
			p->b= 0;
			(*output).push_back(*p);
			p2->points.push_back(*p);

		}
	}

}

void findBegin(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input){

	pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;

	kdtree.setInputCloud (input);

	pcl::PointXYZRGB searchPoint;

	pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp (new pcl::PointCloud<pcl::PointXYZRGB>);

	//TROVO IL PRIMO PUNTO
	for(int i=0; i<input->points.size(); i++){
		if(input->points[i].y> 56 && input->points[i].y< 79 && input->points[i].x> -19 && input->points[i].x< -14 &&
				input->points[i].z < 344 ){

			searchPoint.x= input->points[i].x;
			searchPoint.y= input->points[i].y;
			searchPoint.z= input->points[i].z;

			break;
		}

	}

	// Neighbors within radius search

	std::vector<int> pointIdxRadiusSearch;
	std::vector<float> pointRadiusSquaredDistance;

	float radius = 1;
	double height= 0, y=0;
	int vicini=0;

	bool end= true;

	//TROVO L'INIZIO DEL CAVO
	pcl::PointXYZRGB minPoint;
	minPoint.x= searchPoint.x;
	minPoint.y= searchPoint.y;
	minPoint.z= searchPoint.z;

	pcl::PointXYZRGB maxPoint;
	maxPoint.x= searchPoint.x;
	maxPoint.y= searchPoint.y;
	maxPoint.z= searchPoint.z;


	while(end && searchPoint.x < -2.6){

		height= 0;
		y= 0;

		if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){

			vicini= 0;

			for (int i = 0; i < pointIdxRadiusSearch.size (); ++i){

				double distance;
				float r,g,b;

				//CALCOLO LA DISTANZA DEI COLORI DEL CAVO RISPETTO AL BIANCO
				float diff_r, diff_g, diff_b;
				diff_r= input->points[ pointIdxRadiusSearch[i] ].r - 255;
				diff_g= input->points[ pointIdxRadiusSearch[i] ].g - 255;
				diff_b= input->points[ pointIdxRadiusSearch[i] ].b - 255;


				distance= sqrt(pow( diff_r, 2) +
					pow( diff_g, 2) +
					pow( diff_b, 2));


				//CONSIDERO LA X MAGGIORE PER SPOSTARMI VERSO SINISTRA
				if(minPoint.x < input->points[ pointIdxRadiusSearch[i] ].x){

					minPoint.x= input->points[ pointIdxRadiusSearch[i] ].x;
					minPoint.y= input->points[ pointIdxRadiusSearch[i] ].y;
					minPoint.z= input->points[ pointIdxRadiusSearch[i] ].z;

				}

				vicini++;
				height+= input->points[ pointIdxRadiusSearch[i] ].z;
				y+= input->points[ pointIdxRadiusSearch[i] ].y;

				//SE LA DISTANZA E' INFERIORE AD UNA CERTA SOGLIA, ESCI
				if(distance < 70){
					end=false;
					break;
				}
				else{

					input->points[ pointIdxRadiusSearch[i] ].r= 255;
					input->points[ pointIdxRadiusSearch[i] ].g= 0;
					input->points[ pointIdxRadiusSearch[i] ].b= 0;

				}

			}//for

			//AGGIUSTAMENTO X
			searchPoint.x= minPoint.x;

			//AGGIUSTAMENTO Y
			searchPoint.y= y/vicini;

			//AGGIUSTAMENTO Z
			searchPoint.z= height/vicini;

			searchPoint.r= 255;
			searchPoint.g= 0;
			searchPoint.b= 0;

			input->points.push_back(searchPoint);

		}//if principale

	}

}

bool finalCheck(pcl::PointCloud<pcl::PointXYZRGB>::Ptr p1, pcl::PointCloud<pcl::PointXYZRGB>::Ptr p2, pcl::PointCloud<pcl::PointXYZRGB>::Ptr cavo){

	int internalPoints = 0;

	double minX1, maxX1, minY1, maxY1, minX2, maxX2, minY2, maxY2;

	minX1 = p1->points[0].x;
	maxX1 = p1->points[0].x;
	minY1 = p1->points[0].y;
	maxY1 = p1->points[0].y;

	//TROVO GLI ESTREMI DELL'AREA CONTENENTE IL PIOLO 1
	for(int i = 0; i < p1->points.size(); i++){

		if(p1->points[i].x < minX1){
			minX1 = p1->points[i].x;
		}

		if(p1->points[i].x > maxX1){
			maxX1 = p1->points[i].x;
		}

		if(p1->points[i].y > maxY1){
			maxY1 = p1->points[i].y;
		}

		if(p1->points[i].y < minY1){
			minY1 = p1->points[i].y;
		}

	}

	minX2 = p2->points[0].x;
	maxX2 = p2->points[0].x;
	maxY2 = p2->points[0].y;
	minY2 = p2->points[0].y;

	//TROVO GLI ESTREMI DELL'AREA CONTENENTE IL PIOLO 2
	for(int i = 0; i < p2->points.size(); i++){

		if(p2->points[i].x < minX2){
			minX2 = p2->points[i].x;
		}

		if(p2->points[i].x > maxX2){
			maxX2 = p2->points[i].x;
		}

		if(p2->points[i].y > maxY2){
			maxY2 = p2->points[i].y;
		}

		if(p2->points[i].y < minY2){
			minY2 = p2->points[i].y;
		}

	}

	double maxY, minY;

	if(maxY1 > maxY2)
		maxY = maxY1;
	else
		maxY = maxY2;

	if(minY1 < minY2)
		minY = minY1;
	else
		minY = minY2;

	//CONTROLLO SE IL CAVO PASSA IN MEZZO AI DUE PIOLI
	for(int i=0; i< cavo->points.size(); i++){

		if(cavo->points[i].y < maxY && cavo->points[i].y > minY){

			//PUNTI CHE PASSANO IN MEZZO AI DUE PIOLI
			if(cavo->points[i].x < minX1 && cavo->points[i].x > maxX2){
				internalPoints++;
			}

			//VERIFICO SE IL CAVO PASSA SOPRA IL PIOLO 1
			if(cavo->points[i].x > minX1 && cavo->points[i].x < maxX1 &&
			   cavo->points[i].y > minY1 && cavo->points[i].y < maxY1){
				internalPoints = 0;
				break;
			}

			//VERIFICO SE IL CAVO PASSA SOPRA IL PIOLO 2
			if(cavo->points[i].x > minX2 && cavo->points[i].x < maxX2 &&
			   cavo->points[i].y > minY2 && cavo->points[i].y < maxY2){
				internalPoints = 0;
				break;
			}
		}
	}

	if(internalPoints > 0 ){
		return true;
	}
	else{
		return false;
	}


}



